#pragma once
#include "CSeaAmp.h"
#include"FilterClass.h"
#include "Matrix.h"
#define ANGLE_ADAPTIVE_CONTROL 13
using namespace std;
class AngleAdaptiveControl :
	public CAction
{
public:
	AngleAdaptiveControl(void);
	AngleAdaptiveControl(CSeaAmp &seaamp) {Init(seaamp);}
	~AngleAdaptiveControl(void);
	
	bool Init(CSeaAmp &seaamp) {return CAction::Init(seaamp,ANGLE_ADAPTIVE_CONTROL);}
	const Error *EndableAction(void);
	void DoAct();

	double SetTheta_desire(double theta) {theta_desire=theta;};
	double GetTheta_desire() {return theta_desire;};
	double GetKp(void);
	double GetKd(void);
	void SetKp(double kp);
	void SetKd(double kd);
	double sign(double v);

private:

	void ampLimit(double&,double);
	void sysIdentify(void);
	void sysIdentify_init( void );
	int trackWellNum;
	double Kp;
	double Kd;
	double lamda;
	double a_est;
	double b_est;
	double a3_est;

	double theta_r;
	double theta_rD;
	double theta_rDD;

	double theta_out;
	double theta_outD;
	double theta_out0;
	double theta_out1;
	double theta_out2;
	double theta_out3;

	double theta_desire;
	double theta_desireD;
	double theta_desireDD;
	double theta_desire0;
	double theta_desire1;
	double theta_desire2;
	double theta_desire3;
	double theta_des;
	double theta_desireD0;
	double theta_desireD1;
	double theta_desireD2;
	double theta_desireD3;


	double theta_err;
	double theta_errD;

	double s;
	double torque_est;
	double gama_a;
	double gama_b;
	double gama_a3;
	double torque_PD;
	double torque_limit;
	ofstream Adapative_Data;
	ofstream para;
	double T;
	double dt;
	double torque_total;
	double theta_desireDDL;
	double time;
	double theta_err2;
	double theta_err1;
	double theta_err0;
	double a_estD;
	double b_estD;
	double a3_estD;
	double Pre_torque_total;
	int paraUpdFlag;
	int bigErrNum;
	int smallErrNum;
	double epshro;
	double FrontBackAngle;
	double LeftRightAngle;
	double massCoef;
	double torque_est1;
	double theta_outD1;

	FilterClass filter_theta_out;
	FilterClass filter_theta_outD;
	FilterClass filter_theta_err;
	FilterClass filter_theta_errD;
	FilterClass filter_theta_rD;
	FilterClass filter_theta_rDD;
	FilterClass filter_torque_est;
	double theta_outprev;
	double theta_errprev;
	double theta_out_filter;
	double theta_outD_filter;
	double theta_err_filter;
	double theta_errD_filter;
	double theta_rDprev;
	double theta_rD_filter;
	double theta_rDD_filter;


	double e1;
	double e2;
	double delta_u;
	double u2;
		double u1;
		double ud2;
		double ud1;
		double time_prev;
		double delta[3];
		double torque_PD_pre;
		double paraTimes;

		CMatrix W;
		CMatrix P;
		CMatrix dP;
		double delta_time;
		double sinInterVal;
		double u_pre;
		double theta_errpr;
		CMatrix dA;
		double PredictError;
		CMatrix Sigma;
		CMatrix Rt;
		CMatrix Kt;
		CMatrix Ht;

		CMatrix UnitMat;
		double QQt;
		double var_m;
	};

